#pragma once
#include <math.h>
#include <stdint.h>
#include <mutex>
#include <atomic>
#include "yj_return.hpp"

class pid_pos // 位置式pid_pos
{
private:
    double _Kp;
    double _Kd;
    double _Ki;
    double _set_value;
    double _previous_error;
    double _integral;           // 积分累计量
    double _integral_max_limit; // 积分限幅
    double _integral_min_limit; // 积分限幅
    double _output_max_limit;   // 输出限幅
    double _output_min_limit;   // 输出限幅

    std::atomic<double> _set_value_at;
    std::atomic<double> _now_error_at;
    std::atomic<double> _now_in_value_at;


    std::mutex _data_lock;
    yj_return init_flag;
    bool debug_flag;

public:
    pid_pos(double Kp,
            double Ki,
            double Kd,
            double set_value,
            double integral_max_limit,
            double integral_min_limit,
            double output_max_limit,
            double output_min_limit);

    void reset_value();

    void get_pid_value(double &Kp, double &Ki, double &Kd);
    void set_pid_value(double Kp, double Ki, double Kd);

    double output_limit(double update_value);
    double output_limit(double update_value, double max_limit, double min_limit);

    double update(double value);
    double update_with_I_death_zone(double value, double I_death_value); // 当误差小于I_error_limit时，积分不工作

    void set_output_limit(double max_limit, double min_limit);
    void set_des_value(double set_value);

    double get_des_value();
    double get_error_value();
    double get_update_in_value();

    void open_debug();
    yj_return read_init_status();
};

// 双环PID
class pid_double_ring
{
private:
    pid_pos *_in_pid;  // 内环PID
    pid_pos *_out_pid; // 外环PID

    double _out_pid_max_limit; // 外环PID输出限幅
    double _out_pid_min_limit; // 外环PID输出限幅

    // double _set_value;
    std::mutex _data_lock;

    yj_return init_flag;
    bool debug_flag;

public:
    pid_double_ring(pid_pos *in_pid, pid_pos *out_pid);

    void set_des_value(double set_value);
    void set_out_pid_limit_value(double max_limit, double min_limit);

    double update(double out_value, double in_value);
    double update_with_death_zone(double out_value, double in_value, double in_pid_death_zone);

    double get_des_value();
    double get_error_value();
    double get_update_in_value();

    void open_debug();
    yj_return read_init_status();
};
